#include "GPClientSystem.h"
#include "GPRegisterThreads.h"
#include "GPPackets.h"

#include "raknet/MessageIdentifiers.h"
#include "raknet/RakNetworkFactory.h"
#include "raknet/RakNetTypes.h"

/*!
 * \brief The constructor of the client system.
 * 
 * \param server
 * The address of the server to which to connect to.
 * 
 * \param port
 * The port number to be used.
 * 
 * This constructor stores the server address and the port number. It
 * sets the visual system to NULL, which means that it does not have
 * a visual system which it should update. The user should call the 
 * function setVisualSystem of this class to set it. It also sets the 
 * lastupdate_ time to the current time.
 *
 * \see
 * setVisualSystem
 */
GPClientSystem::GPClientSystem(std::string server, int port) : GPThreadedSystem(true), server_(server), port_(port), visual_(NULL) {
	lastupdate_ = getTime();
	peer_ = NULL;
}

/*!
 * \brief The default destructor.
 */
GPClientSystem::~GPClientSystem() {
}

/*!
 * \brief Set the visual system for the client to update.
 * 
 * \param visual
 * This is the pointer to the visual system that needs to be updated.
 */
void GPClientSystem::setVisualSystem(GPVisualSystem* visual) {
	visual_ = visual;
}

/*!
 * \brief The initialization function of the client
 * 
 * \returns true if the initialization succeeds
 * 
 * This function initialized the client system. It first creates a peer interface,
 * which it then starts. After this has been succesful, the system tries to 
 * connect to the server. After waiting a second, it tests whether this have been
 * succesful. A more efficient mechanism would be to send acknowledge packets
 * which eleviates the need to check wether the server and client are connected.
 */
bool GPClientSystem::initialize() {

	estimationOrder_= atoi(settings_["prediction"].c_str());
	convergenceOrder_= atoi(settings_["convergence"].c_str());
	convtime_= atoi(settings_["convtime"].c_str());

	// Create a new peer interface to facilitate the client-server connection.
	peer_ = RakNetworkFactory::GetRakPeerInterface();
	
	// Start the peer interface. The update frequency of the
	//  socket thread is 5 microseconds. And this client
	//  is only allowed to connect to 1 server.
	if (!(peer_->Startup(1, 5, &SocketDescriptor(), 1)))
		return false;

	// Try to connect to the server
	if (!(peer_->Connect(server_.c_str(), port_, 0, 0)))
		return false;

	// Wait to a second for the client to connect to the server
	microSleep(1000000);

	// Check whether the client and server are connected
	return peer_->IsConnected( peer_->GetSystemAddressFromIndex(0));
}

/*!
 * \brief When a packet is received, this function is called.
 * 
 * \param packet The packet that is received.
 *
 * When a packet is received, this function handles it. There
 * are several packet types that the client accepts.
 *
 * \see 
 * ID_DISCONNECTION_NOTIFICATION | ID_CREATE_BOX | ID_CREATE_SPHERE | ID_UPDATE_STATE
 */
void GPClientSystem::receivePacket(Packet* packet) {

	int objectID;
	CreateSpherePacket* csp;
	CreateBoxPacket*    cbp;
	UpdateStatePacket*  usp;
	PredictionData *predictionPacket=new PredictionData;

	// if the packet is a disconnect notification, close the application.
	if ((unsigned char)packet->data[0]==ID_DISCONNECTION_NOTIFICATION) {
		std::cout << "The server has disconnected." << std::endl;
		exit(-1);
	}

	// only handle other packets if the visual system is set
	if (visual_!=NULL) {

		// switch to the correct packet type...
		switch( (unsigned char)packet->data[0] ) {

			// create box packet is received when the server has created a box in the world
			case ID_CREATE_BOX: 
				// cast the packet to the correct packet
				cbp = (CreateBoxPacket*)(packet->data);

				// the create box function in the visual system is already thread-safe
				objectID = visual_->createBox(cbp->w, cbp->h, cbp->d, cbp->x, cbp->y, cbp->z, cbp->qx, cbp->qy, cbp->qz, cbp->qw, cbp->vx,cbp->vy,cbp->vz, cbp->wx,cbp->wy,cbp->wz, 0, cbp->material);
					
				// store the object id...
				objectToVisualIDs_[cbp->objectID] = objectID;

			
				//std::cout<<" CUbe "<<cbp->objectID<<","<<cbp->dynamic <<std::endl;
			   //create a data struct
				
				if(cbp->dynamic==true)
				{
				predictionPacket->lastupdate = getTime();
				predictionPacket->x = cbp->x;
				predictionPacket->y = cbp->y;
				predictionPacket->z = cbp->z;
				predictionPacket->vx = cbp->vx;
				predictionPacket->vy = cbp->vy;
				predictionPacket->vz = cbp->vz;
				predictionPacket->wx = cbp->wx;
				predictionPacket->wy = cbp->wy;
     			predictionPacket->wz = cbp->wz;
	
					

				predictionPacket->ax = 0;
				predictionPacket->ay = 0;
				predictionPacket->az = 0;

				predictions_[cbp->objectID] = predictionPacket;
				
				}
				visual_->setPosition(cbp->objectID,cbp->x,cbp->y,cbp->z);
				break;

			// create sphere packet is received when the server has created a sphere in the world
			case ID_CREATE_SPHERE:	
				// cast the packet to the correct packet
				csp = (CreateSpherePacket*)(packet->data);

				// the create sphere function in the visual system is already thread-safe
				objectID = visual_->createSphere(csp->r, csp->x, csp->y, csp->z, csp->qx, csp->qy, csp->qz, csp->qw, csp->vx,csp->vy,csp->vz, csp->wx,csp->wy,csp->wz, 0, csp->material);

				// store the object id...
				objectToVisualIDs_[csp->objectID] = objectID;
				   //create a data struct
				//std::cout<<" shpere "<<csp->objectID<<","<<csp->dynamic <<std::endl;

				if(csp->dynamic==true)
				{
				predictionPacket->lastupdate = getTime();
				predictionPacket->x = csp->x;
				predictionPacket->y = csp->y;
				predictionPacket->z = csp->z;
				predictionPacket->vx = csp->vx;
				predictionPacket->vy = csp->vy;
				predictionPacket->vz = csp->vz;
				predictionPacket->wx = csp->wx;
				predictionPacket->wy = csp->wy;
     			predictionPacket->wz = csp->wz;
				predictionPacket->ax = 0;
				predictionPacket->ay = 0;
				predictionPacket->az = 0;
					
				predictions_[csp->objectID] = predictionPacket;
				

				}
				visual_->setPosition(csp->objectID,csp->x,csp->y,csp->z);
				break;

			// the state of an object in the simulation has changed, therefore an update packet
			// has been sent.
			case ID_UPDATE_STATE:
				// cast the packet to the correct packet
				usp = (UpdateStatePacket*)(packet->data);
				//std::cout << "Update: "<<usp->objectID<<", client"<<", x "<< usp->x <<" y "<< usp->y <<" z "<< usp->z << std::endl;
				// if the object is in the visual system
				if (objectToVisualIDs_.find(usp->objectID)!=objectToVisualIDs_.end()) {
				
					// make sure this is done thread-safe
					synchronizeEnter();

					// put the update in the queue
					updates_.push(*usp);

					// leave the critical section
					synchronizeLeave();
				}
				break;
		
			// a ground plane has been created.
			case ID_CREATE_GROUNDPLANE:
			
				// make it thread-safe
				visual_->synchronizeEnter();
			
				// create the ground plane in the visual system
				visual_->createGroundPlane();

				// leave the critical section
				visual_->synchronizeLeave();
				break;
		}
	}
}

/*!
 * \brief Send a packet to the server
 * 
 * \param packet
 * The packet to be send, casted to a const char*
 * 
 * \param size
 * The size in bytes of the message to be send
 * 
 * \param systemAddress
 * The address to which this should be send.
 */
void GPClientSystem::sendPacket(const char* packet, const int size, SystemAddress systemAddress) {
	// send the packet...
	peer_->Send(packet, size, HIGH_PRIORITY, RELIABLE_ORDERED, 0, systemAddress, false); 
}


/*!
 * \brief This function creates and send the request to create a box on the server.
 * 
 * \param w The width of the box
 * \param h The height of the box
 * \param d The depth of the box
 * \param x The x position of the box
 * \param y The y position of the box
 * \param z The z position of the box
 * \param qx The x component of the quaternion describing the orientation of the box
 * \param qy The y component of the quaternion describing the orientation of the box
 * \param qz The z component of the quaternion describing the orientation of the box
 * \param qw The w component of the quaternion describing the orientation of the box
 * \param vx The x component of the linear velocity
 * \param vy The y component of the linear velocity
 * \param vz The z component of the linear velocity
 * \param wx The x component of the angular velocity
 * \param wy The y component of the angular velocity
 * \param wz The z component of the angular velocity
 * \param mass The mass of the object
 * \param dynamic This parameter indicates whether the object is dynamic or static
 *
 * \returns 0
 *
 * This function creates a packet with the information for creating a box. This packet
 *  will be send to the server. The server then create the box and sends a packet back
 *  such that the client can create the object in the visual system.
 */
int GPClientSystem::createBox(float w, float h, float d,
							  float x, float y, float z,
							  float qx, float qy, float qz, float qw,
							  float vx, float vy, float vz,
							  float wx, float wy, float wz, 
							  float mass, bool dynamic) 
{
	// the request packet
	RequestCreateBoxPacket cbp;
	
	// all parameters are set
	cbp.w  = w;  cbp.h  = h;  cbp.d  = d;
	cbp.x  = x;  cbp.y  = y;  cbp.z  = z;
	cbp.vx = vx; cbp.vy = vy; cbp.vz = vz;
	cbp.wx = wx; cbp.wy = wy; cbp.wz = wz;
	cbp.qx = qx; cbp.qy = qy; cbp.qz = qz; cbp.qw = qw;
	cbp.mass     = mass; 
	cbp.dynamic  = dynamic; 
	cbp.objectID = -1;
	
	// the type of the packet is set
	cbp.typeID   = ID_REQ_CREATE_BOX;

	// the packet is send to the first address to which the client is connected
	sendPacket((const char*)(&cbp), sizeof(cbp), peer_->GetSystemAddressFromIndex(0));
	
	return 0;
}

/*!
 * \brief This function creates and send the request to create a sphere on the server.
 * 
 * \param r The radius of the sphere
 * \param x The x position of the sphere
 * \param y The y position of the sphere
 * \param z The z position of the sphere
 * \param qx The x component of the quaternion describing the orientation of the sphere
 * \param qy The y component of the quaternion describing the orientation of the sphere
 * \param qz The z component of the quaternion describing the orientation of the sphere
 * \param qw The w component of the quaternion describing the orientation of the sphere
 * \param vx The x component of the linear velocity
 * \param vy The y component of the linear velocity
 * \param vz The z component of the linear velocity
 * \param wx The x component of the angular velocity
 * \param wy The y component of the angular velocity
 * \param wz The z component of the angular velocity
 * \param mass The mass of the object
 * \param dynamic This parameter indicates whether the object is dynamic or static
 *
 * \returns 0
 *
 * This function creates a packet with the information for creating a sphere. This packet
 *  will be send to the server. The server then create the sphere and sends a packet back
 *  such that the client can create the object in the visual system.
 */
int GPClientSystem::createSphere(float r,
								 float x, float y, float z,
								 float qx, float qy, float qz, float qw,
								 float vx, float vy, float vz,
								 float wx, float wy, float wz, 
								 float mass, bool dynamic)
{
	// the request packet
	RequestCreateSpherePacket csp;

	// all parameters are set
	csp.r = r;
	csp.x  = x;  csp.y  = y;  csp.z  = z;
	csp.vx = vx; csp.vy = vy; csp.vz = vz;
	csp.wx = wx; csp.wy = wy; csp.wz = wz;
	csp.qx = qx; csp.qy = qy; csp.qz = qz; csp.qw = qw;
	csp.mass     = mass;
	csp.dynamic  = dynamic;
	csp.objectID = -1;

	// the type of the packet is set
	csp.typeID   = ID_REQ_CREATE_SPHERE;

	// the packet is send to the first address to which the client is connected
	sendPacket((const char*)(&csp), sizeof(csp), peer_->GetSystemAddressFromIndex(0));

	return 0;
}


/*!
 * \brief This function removes all spaces at the beginning and the end of a string
 * \param value The string that should be stripped from all leading and final spaces.
 */
extern void trimspaces(std::string& value);

/*!
 * \brief Reads a settings file.
 * 
 * \param filename The name of the file containing the settings
 * 
 * This function reads a file specified by the filename argument. The
 * file is a \<property\>=\<value\> file. If the first character of a line is
 * a \#, the line is considered a comment line.
 */
void GPClientSystem::loadSettings(const std::string& filename) {
	// first clear the existing settings
	settings_.clear();
	// create an input stream
	std::ifstream settingsfile(filename.c_str());
	// the string variable containing the next line
	std::string line;
	// while the file has not reached its end...
	while (std::getline(settingsfile, line)) {
		// ... check if the line is not a comment line ...
		if (line.c_str()[0]!='#') {
			// ... and find the '=' sign.
			size_t pos = line.find_first_of("=");
			std::string param, value;
			// get the first part (property/param) of the string
			param = line.substr(0,pos);  trimspaces(param);
			// get the last part (value) of the string
			value = line.substr(pos+1);  trimspaces(value);
			// store the param/value pair in the settings_ variable
			settings_[param]=value;
		}
	}
	// finally, close the input stream
	settingsfile.close();
}


/*!
 * \brief The actual function that is called when running the thread.
 * 
 * This is the real function of the client system. Here, the packets
 * are received and handled.
 */
void GPClientSystem::run() {
	// set the update frequency
	float delta_t = 0.02;
	// register the thread with Ogre
	doRegisterThread();

	// wait for the system to initialize
	while (!initialized_) microSleep(1000);

	// while the client system does not have to stop do...
	while (!hasToStop_)
	{
		// receive a packet
		Packet *packet = peer_->Receive();
		// only when a packet is received handle it
		if (packet) receivePacket(packet);
		// and deallocate the packet.
		peer_->DeallocatePacket(packet);
		
		// if it is time to batch-handle the state update packets...
		if (diffWalltime(lastupdate_, getTime()) > delta_t) 
		{
			float passedTime = diffWalltime(lastupdate_, getTime());

			lastupdate_ = getTime();
			
			// make sure this is done thread-safe
			synchronizeEnter();
			visual_->synchronizeEnter();

			//iterator through the estimations
			std::map<int, PredictionData*>::iterator iter;
			float x,y,z;
			//float q1,q2,q3,q4;
			
			//For all  predictions
			for(iter = predictions_.begin(); iter != predictions_.end(); iter++){
				visual_->getPosition(iter->first, x, y, z);
				
				//nomalization of angular velocity used to estimate the rotation
				//float normAngular = sqrt( pow(iter->second->wx, 2) + pow(iter->second->wy, 2) + pow(iter->second->wz, 2));

				//if convergence is on , and the period is still valid
				if(convergence_.find(iter->first) != convergence_.end() && convergence_[iter->first]->valid == 1)
				{
					ConvData* conv = convergence_[iter->first];
					//converge the data
					visual_->setPosition(iter->first, x + (conv->vx*passedTime),y + (conv->vy*passedTime), z + (conv->vz*passedTime));
					//Update the time needed to converge and check if this data should still be used
					conv->timeToConv -= passedTime;
					if(conv->timeToConv <= 0)
					{
						//period is over
						conv->valid = 0;
					}
				}
				else if(estimationOrder_ == 1)//linear
				{
					visual_->setPosition(iter->first, x + (iter->second->vx*passedTime), y + (iter->second->vy*passedTime), z + (iter->second->vz*passedTime));
					//std::cout <<"C-Obj:"<<iter->first  << ", x " << x + (iter->second->vx*passedTime) << " y "<< y + (iter->second->vy*passedTime) << " z " << z + (iter->second->vz*passedTime)<<std::endl;
					//orientation prediction
					/*q1 = cos(normAngular*(passedTime/2)) * iter->second->qx  -  (iter->second->wx/ normAngular) * sin(normAngular*(passedTime/2))* iter->second->qy -  (iter->second->wy/ normAngular) * sin(normAngular*(passedTime/2))* iter->second->qz -  (iter->second->wz/ normAngular) * sin(normAngular*(passedTime/2))* iter->second->qw;
					q2 = cos(normAngular*(passedTime/2)) * iter->second->qx  +  (iter->second->wx/ normAngular) * sin(normAngular*(passedTime/2))* iter->second->qy -  (iter->second->wy/ normAngular) * sin(normAngular*(passedTime/2))* iter->second->qz +  (iter->second->wz/ normAngular) * sin(normAngular*(passedTime/2))* iter->second->qw;
					q3 = cos(normAngular*(passedTime/2)) * iter->second->qx  +  (iter->second->wx/ normAngular) * sin(normAngular*(passedTime/2))* iter->second->qy +  (iter->second->wy/ normAngular) * sin(normAngular*(passedTime/2))* iter->second->qz -  (iter->second->wz/ normAngular) * sin(normAngular*(passedTime/2))* iter->second->qw;
					q4 = cos(normAngular*(passedTime/2)) * iter->second->qx  -  (iter->second->wx/ normAngular) * sin(normAngular*(passedTime/2))* iter->second->qy +  (iter->second->wy/ normAngular) * sin(normAngular*(passedTime/2))* iter->second->qz +  (iter->second->wz/ normAngular) * sin(normAngular*(passedTime/2))* iter->second->qw;
					visual_->setOrientation(iter->first, q1,q2,q3,q4);*/
				
				}
				else if(estimationOrder_ == 2)//quadratic
				{ 
					visual_->setPosition(iter->first, x + (iter->second->vx*passedTime) + (0.5*iter->second->ax*passedTime*passedTime), 
						y + (iter->second->vy*passedTime) + (0.5*iter->second->ay*passedTime*passedTime), 
						z + (iter->second->vz*passedTime) + (0.5*iter->second->az*passedTime*passedTime));
				}
			}

			// while there are still updates in the queue
			while (!updates_.empty())
			{
				// get the first one
				UpdateStatePacket p = updates_.front();
				
				// retreive the visual id from the object id
				int objectID = objectToVisualIDs_[p.objectID];
				visual_->setOrientation(objectID, p.qx, p.qy, p.qz, p.qw);
				// set the position and orientation

				if(convergenceOrder_ == 0)//snap 
				{ 
					//set the visual position to the received data directly
					visual_->setPosition(objectID, p.x, p.y, p.z);
					visual_->setOrientation(objectID, p.qx,p.qy,p.qz,p.qw);
				}
				else if(convergenceOrder_ == 1) //linear
				{ 
					visual_->getPosition(objectID, x, y, z);
					//Add the created data to the map
					if(convergence_.find(objectID) == convergence_.end())
					{
						convergence_[objectID] = new ConvData;
					}

					float destx, desty, destz;
					convergence_[objectID]->timeToConv = convtime_;
					//Code below , calculate the position where the client should meet the received server data
					if(estimationOrder_ == 1)
					{
						destx = p.x + p.vx*convtime_;
						desty = p.y + p.vy*convtime_;
						destz = p.z + p.vz*convtime_;
					}
					else if(estimationOrder_ == 2)
					{
						destx = p.x + p.vx*convtime_ + 0.5*p.ax*convtime_*convtime_;
						desty = p.y + p.vy*convtime_ + 0.5*p.ay*convtime_*convtime_;
						destz = p.z + p.vz*convtime_ + 0.5*p.az*convtime_*convtime_;
					}
					//Convergence speed needed to reach the goal on the specific time
					convergence_[objectID]->vx = (destx - x) / convtime_;
					convergence_[objectID]->vy = (desty - y) / convtime_;
					convergence_[objectID]->vz = (destz - z) / convtime_;
					//Indicate this data is valid and thus it should be used
					convergence_[objectID]->valid = 1;
				}

				//if the estimation does not exits add it.
				if(predictions_.find(objectID) == predictions_.end()){
					predictions_[objectID] = new PredictionData;
				}

				//fill estimations data with the recieved data from the server
				predictions_[objectID]->vx = p.vx;
				predictions_[objectID]->vy = p.vy;
				predictions_[objectID]->vz = p.vz;
				predictions_[objectID]->qx = p.qx;
				predictions_[objectID]->qy = p.qy;
				predictions_[objectID]->qz = p.qz;
				predictions_[objectID]->qw = p.qw;
				predictions_[objectID]->ax = p.ax;
				predictions_[objectID]->ay = p.ay;
				predictions_[objectID]->az = p.az;
				predictions_[objectID]->wx = p.wx;
				predictions_[objectID]->wy = p.wy;
				predictions_[objectID]->wz = p.wz;
				
				updates_.pop();			
			}

			// release the visual and client systems
			visual_->synchronizeLeave();
			synchronizeLeave();
		}
	}

	// the peer interface should be shutdown properly
	peer_->Shutdown(300);
	// and destroy the peer interface.
	RakNetworkFactory::DestroyRakPeerInterface(peer_);
	peer_ = NULL;
}